import numpy as np
from bins.vehicleParams import VehicleParams
from src.common.processedmeasurement import ProcessedMeasurement

a = ProcessedMeasurement()
dtype = np.dtype([
            ('x', np.float32),
            ('y', np.float32),
            ('h', np.float32),
            ('v', np.float32)
        ])
traj_file = 'trajeo.txt'
traj_data = np.loadtxt(traj_file, delimiter=' ', dtype=dtype)
# 初始化车辆轨迹
a.init_traj(traj_data)

current_state = np.zeros(1, dtype=dtype)
current_state[0] = (18.274, 36.169, 0, 0)
current_state = [82.21, 21.816, 0, 0, 0]
print(current_state)
# 更新车辆当前位置
a.update_state(current_state)
# 计算车辆下一步参考点
a.calculate_ref_point()
print(a.current_state)
print(a.match_state)
print(a.nearest_index)
print(a.get_predict_traj())
print(a.calculate_current_state)

print(a.log_data)
# print(a.traj_point)

# a = VehicleParams()
# a.load_vehicle_params()
# print(a.vehicle_params)

